#ifndef _UBT_STATE_TASK_NAV_H_
#define _UBT_STATE_TASK_NAV_H_
#include "ubt_state_types.h"
#include "ubt_state_api.h"
#include "ubt_interface/msg/nav_path_info.hpp"
#include "ubt_interface/msg/line.hpp"
#include "ubt_interface/msg/limit.hpp"
#include "ubt_interface/msg/point.hpp"
#include "ubt_interface/msg/nav_task_limit.hpp"
namespace ubt_state
{
class CTaskNav: public ITask
{
  public:
    static int ClassInit( ObstacleParams obsParams);
    static void ClassCleanup();
    static ITask *CreateTask(ITaskCb *pTaskCb, std::string taskId, std::vector<Line> *_pLineInfos, ubt_task_stage stage, int prePathAvoidHwMask, int index, int reportIndex);

    ~CTaskNav();

    bool Init(ITaskCb *pTaskCb, std::string taskId, std::vector<Line> *_pLineInfos, ubt_task_stage stage, int prePathAvoidHwMask, int index, int reportIndex);

    virtual bool Start();
    virtual void Pause();
    virtual void Continue();
    virtual void Stop(int reason);
    virtual void OnTimer();
    virtual void OnMsg(int msgType, void *pvMsg);

    virtual ubt_task_type GetType();
    virtual ubt_task_state GetState();
    virtual ubt_task_stage GetStage();
    virtual std::string GetActionType();
    virtual void GetIndex(int& index, int& reportIndex);
    virtual void GetStopInfo(int& stopReason, std::string& stopReasonDesc);
    virtual void GetLogs(std::vector<TaskLogItem>& logs);

    void Fixedpath_Status_Callback(const std_msgs::msg::String::SharedPtr msg);
#ifdef AVOID_CAR_SUPPORT
    void SetCarParams(float checkDis, int waitTime);
    void CarPose_Callback(const geometry_msgs::msg::Pose2D::SharedPtr msg);
#endif

  private:
    void _OnLocationCb();
    void _OnAvoidStatusCb();
    void _OnArrive(int pathIndex);
    void _ApplyLineProp(int pathIndex);
    void _CheckObstacle(int event);
    void _SetState(ubt_task_state newState, int reason, std::string reasonStr);
#ifdef AVOID_CAR_SUPPORT
    void _NotifyCarStateChanged(long now);
#endif
    int _SendNavInfo();
    int _Line2Nav(ubt_interface::msg::Line& navLine, Line *pLine);
    void _PubNavLimit(NavTaskLimitIndex *taskLimit);

    ITaskCb *_pTaskCb;
    std::string _taskId;
    int _index;
    int _reportIndex;
    std::vector<Line> *_pLineInfos;
    ubt_task_stage _stage;
    int _prePathAvoidHwMask;
    int _waitTime;

    ubt_task_state _state;
    long _stateTime;
    int _flag;//0x01 load_path send

    int _stopReason;
    std::string _stopReasonDesc;


    int  _obstacleState;//避障状态,0-不限,1 急停，2减速 ，3 报警
    long  _obstacleStateStime;//避障状态开始时间
    int _avoidHwStatus;	//硬件避障状态
    int  _avoidDisable;//避障是否关闭
    int _avoidStage;//0 检测即将到达，1 检测到达,2  检测出发,

    int _pathIndex;//当前线段
    std::string _oldMsgData;
    int _closedReason;	//进入closed状态的原因
    std::string _closedReasonStr;//原因描述

#ifdef AVOID_CAR_SUPPORT
    float _checkDis2;	//检测小火车距离^2，单位米

    int       _carState;//小火车状态,0 无，1 有
    long      _carStateTime;//小火车状态的开始时间
    geometry_msgs::msg::Pose2D _carStartPose; //小火车位姿
    geometry_msgs::msg::Pose2D _carCurPose; //小火车位姿
    long      _carLastReportTm;//小火车状态最近上报时间
#endif
};
}
#endif

